#include "camerawidget.h"

CameraWidget::CameraWidget(Camera *camera, QBoxLayout *robotLayout, QWidget *parent) : QWidget(parent)
{
    this->camera = camera;
    this->robotLayout = robotLayout;
    QVBoxLayout *layout = new QVBoxLayout;
    cvwidget = new QOpenCVWidget(this);
    layout->addWidget(cvwidget);
    setLayout(layout);
    //resize(640, 480);

    QTimer *timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(reprint()));
    timer->start(100);
}

double CameraWidget::grad2rad(double grad) {
        return (grad*M_PI/180);
}

CvPoint CameraWidget::robotEndPointRad(CvPoint center, double radian, int robotRadius)
{
        return cvPoint(center.x + (robotRadius * cos(radian)), center.y + (robotRadius * -sin(radian)));
}

CvPoint CameraWidget::robotEndPointGrad(CvPoint center, double grad, int robotRadius)
{
        return robotEndPointRad(center, grad2rad(grad), robotRadius);
}

IplImage* CameraWidget::printObject(IplImage* image, int x, int y, double radian, CvScalar color, int robotRadius, int lineWidth, int trans)
{
        if(trans == 1){
                IplImage* imDup = cvCloneImage(image);
                cvZero(imDup);

                CvPoint center = cvPoint(x, y);
                cvCircle(imDup, center, robotRadius, color, lineWidth, 8, 0);
                cvLine(imDup, center, robotEndPointRad(center, radian, robotRadius), color, lineWidth , CV_AA, 0);

                cvAdd(imDup, image, image);
                cvReleaseImage(&imDup);
                return image;
        }

        CvPoint center = cvPoint(x, y);
        //1/3 Drawing Circle
        cvCircle(image, center, robotRadius, color, lineWidth, 8, 0);
        //2/3 Drawing line for direction
        cvLine(image, center, robotEndPointRad(center, radian, robotRadius), color, lineWidth , CV_AA, 0);
        //3/3 Drawing center point
        cvLine(image, center, center, color, lineWidth , CV_AA, 0);
        return image;
}
void CameraWidget::reprint() {
    //Load Images
    //const char* printObjectFileName = "roboterplatz.bmp";

    IplImage* printObjectImage = camera->getImage();

    ObjektListe * ol = camera->getPositions();
    if(ol != NULL)
    {
        for (int i = 0; i < ol->anzahlObjekte; i++)
        {

                    Kordinate pos = ol->kord[i];



                    //gedachte Position zeichnen
                    printObject(printObjectImage, pos.xkordinate_mm, pos.ykordinate_mm,
                                CV_PI*pos.theta_grad/180, cvScalar(255, 170, 170, 0),
                                10, 3, 0);

        }
    }

    RobotWidget *robotWidget;
    for (int i = 0; i < robotLayout->count(); i++)
    {
        if (robotLayout->itemAt(i)->widget())
        {
            robotWidget = dynamic_cast<RobotWidget*> (robotLayout->itemAt(i)->widget());
            if (robotWidget)
            {
                Kordinate *truePos = robotWidget->getRobot()->getTruePosition();
                Kordinate *evalPos = robotWidget->getRobot()->getEvalPosition();
                QColor *color = robotWidget->getRobot()->getColor();

                //richtige Position zeichnen
                printObject(printObjectImage, truePos->xkordinate_mm, truePos->ykordinate_mm,
                            CV_PI*truePos->theta_grad/180, cvScalar(color->blue(), color->green(), color->red(), 0),
                            10, 3, 0);

                //gedachte Position zeichnen
                printObject(printObjectImage, evalPos->xkordinate_mm, evalPos->ykordinate_mm,
                            CV_PI*evalPos->theta_grad/180, cvScalar(50, 50, 50, 0),
                            10, 3, 0);

            }
        }
    }


    cvwidget->putImage(printObjectImage);

    //clean up images
    //cvReleaseImage(&printObjectImage);
}
